﻿/*******************************************************************************
 * AAEngine
 * Copyright (c) 2010 Mike Jarosch
 *
 * Original source PushButton Engine:
 * Copyright (C) 2009 PushButton Labs, LLC
 * For more information see http://www.pushbuttonengine.com
 *
 * This file is licensed under the terms of the MIT license, which is included
 * in the "MIT License.txt" file at the root directory of this SDK.
 ******************************************************************************/

using System.Collections.Generic;
using System.Linq;

namespace AAEngine.Engine.Core
{
    /// <summary>
    /// A priority queue to manage prioritized data.     
    /// The implementation is based on the heap structure.
    /// </summary>
    /// <typeparam name="T">The type of the object we are storing.</typeparam>
    public class SimplePriorityQueue<T> where T : IPrioritizable
    {
        T[] _heap;
        Dictionary<T, int> _positionLookup;
        int _size, _count;

        public SimplePriorityQueue(int capacity)
        {
            _size = capacity + 1;
            _heap = new T[_size];
            _positionLookup = new Dictionary<T, int>();
        }

        public T Front
        {
            get
            {
                return _heap[1];
            }
        }

        public int Size
        {
            get
            {
                return _size;
            }
        }

        public bool Enqueue(T obj)
        {
            if (_count + 1 < _size)
            {
                _count++;
                _heap[_count] = obj;
                _positionLookup[obj] = _count;
                walkUp(_count);
                return true;
            }
            return false;
        }

        public T Dequeue()
        {
            if (_count >= 1)
            {
                var o = _heap[1];
                _positionLookup.Remove(o);
                _heap[1] = _heap[_count];
                walkDown(1);
                _heap[_count] = default(T);
                _count--;
                return o;
            }
            return default(T);
        }

        public bool Reprioritize(T obj, int newPriority)
        {
            if (!_positionLookup.ContainsKey(obj))
                return false;

            var oldPriority = obj.Priority;
            obj.Priority = newPriority;
            var position = _positionLookup[obj];
            if (newPriority > oldPriority)
            {
                walkUp(position);
            }
            else
            {
                walkDown(position);
            }
            return true;
        }

        public bool Remove(T obj)
        {
            if (_count >= 1)
            {
                var position = _positionLookup[obj];
                var o = _heap[position];
                _positionLookup.Remove(o);
                _heap[position] = _heap[_count];
                walkDown(position);

                _heap[_count] = default(T);
                _count--;

                return true;
            }
            return false;
        }

        public bool Contains(T obj)
        {
            for (var i = 1; i <= _count; i++)
            {
                if (_heap[i].Equals(obj))
                    return true;
            }
            return false;
        }

        public void Clear()
        {
            _heap = new T[_size];
            _positionLookup.Clear();
            _count = 0;
        }

        public int Count
        {
            get { return _count; }
        }

        public bool IsEmpty
        {
            get { return _count == 0; }
        }

        public T[] ToArray()
        {
            return _heap.Skip(1).Take(_count).ToArray();
        }

        //public string ToString()
        //{
        //    return "[SimplePriorityQueue size=" + _size + "]";
        //}

        void walkUp(int index)
        {
            var parent = index >> 1;
            T parentObject;
            var tmp = _heap[index];
            var p = tmp.Priority;

            while (parent > 0)
            {
                parentObject = _heap[parent];
                if (p - parentObject.Priority > 0)
                {
                    _heap[index] = parentObject;
                    _positionLookup[parentObject] = index;
                    index = parent;
                    parent >>= 1;
                }
                else
                    break;
            }

            _heap[index] = tmp;
            _positionLookup[tmp] = index;
        }

        void walkDown(int index)
        {
            var child = index << 1;
            T childObject;

            var tmp = _heap[index];
            var p = tmp.Priority;

            while (child < _count)
            {
                if (child < _count - 1)
                {
                    if (_heap[child].Priority - _heap[child + 1].Priority < 0)
                        child++;
                }

                childObject = _heap[child];

                if (p - childObject.Priority < 0)
                {
                    _heap[index] = childObject;
                    _positionLookup[childObject] = index;
                    _positionLookup[tmp] = child;
                    index = child;
                    child <<= 1;
                }
                else
                    break;
            }
            _heap[index] = tmp;
            _positionLookup[tmp] = index;
        }
    }
}
